#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "Car.h"
//#include "Serial.h"
#include "mpu6050.h"
#include "PID.h"
//#include "Timer.h"
#include "math.h"

int16_t AX,AY,AZ,GX,GY,GZ;
float PID_P[5] = {4,0.5,5,30,80};		//kp,ki,kd,maxIntegral,maxOut
float g_fBalance_Gyro,g_fAccel_Angle;
float g_fBalance_Angle;

int main(void)
{
	uint8_t gyro_offset = 1;	//静置时角速度的偏移量
	float gyro_dt = 0.004f;     //陀螺仪角速度积分系数，增长缓慢就增加
	float Filter_Weight = 0.02; //滤波权重
	
	PID_Init(&mypid,PID_P);
	Car_Init();
//	Serial_Init();
	MPU6050_Init();
	
	while(1)
	{
		MPU6050_GetData(&AX,&AY,&AZ,&GX,&GY,&GZ);
		
		g_fBalance_Gyro = GY - gyro_offset;//原始角速度减去零偏值得到的实际角速度
		g_fAccel_Angle = (float)atan2(AX,AZ)*57.296;//两轴加速度求反三角得到加速度角度，乘以57.296，把弧度转化为度
		//一阶互补滤波核心公式
		g_fBalance_Angle = Filter_Weight*g_fAccel_Angle+(1-Filter_Weight)*(g_fBalance_Angle-g_fBalance_Gyro*gyro_dt);

//		if(Serial_RxFlag == 0)
//		{
			mypid.output = PID_Calc(&mypid,0.0f,g_fBalance_Angle);
			Delay_ms(10);
			if(mypid.output>0)
				Car_Go((int)mypid.output);
			else 
				Car_Back( (int)(-mypid.output) );				 //反转		
//		}
	}
}
